% ! problem8.m --> based on <<MATLAB codes for Finite Element Analysis>>
% * based on problem7.m file , whose some parameter ared modified 

clear ;
clc
format default
p=struct();

% define model parameters
% unit:SI(mm)
p.E = 210000;
p.A = 100;
p.P = -10000;
disp('单位制:SI(mm)')
% define elems and nodes

p.nodes=[4 4 3 ;  % node's coordinates (x,y,z)
        0 4 0;
        0 4 6;
        4 0 3;
        8 -1 1];
p.elems=[1 2 ; 
        1 3;
        1 4;
        1 5];

p.node_num=size(p.nodes,1);
p.elem_num=size(p.elems,1);
p.node_Coord_x=p.nodes(:,1);
p.node_Coord_y=p.nodes(:,2);
p.node_Coord_z=p.nodes(:,3);

% define problem's dimension : 1D/2D/3D;
%        = each node's dof  
p.problem_dimension=size(p.nodes,2);

% global degree of freedom number
p.global_dof_num=p.problem_dimension*p.node_num;
% define global Nodal displacement  colum vector
p.displacements=zeros(p.global_dof_num,1);
p.node_forces=zeros(p.global_dof_num,1);

% define boundary conditions
% fixed dof 
p.fix_dof=[4:15]';
% load dof and amplitude
p.load_dof=2;

% initial global stiffness matrix
p.global_stiffness_matrix=zeros(p.global_dof_num);

% compute all ElemStiffnessMatrix and assembly stiff matrix
elem_global_dof_num=p.problem_dimension*size(p.elems,2);
p.elemStiffs=zeros(elem_global_dof_num,elem_global_dof_num,p.elem_num);

for i=1:p.elem_num
    connectivity=p.elems(i,:);
    % elem's all dof : a 3d truss node has 2 node,each node have 3 dof,
    % i-th node --> U_(3*i-2) / U_(3*i-1) / U_(3*i) dof(displacement of node) 
    elem_dof=[connectivity(1)*3-2 connectivity(1)*3-1 connectivity(1)*3 ...
        connectivity(2)*3-2 connectivity(2)*3-1 connectivity(2)*3];

    node1_x=p.node_Coord_x(connectivity(1));
    node1_y=p.node_Coord_y(connectivity(1));
    node1_z=p.node_Coord_z(connectivity(1));
    node2_x=p.node_Coord_x(connectivity(2));
    node2_y=p.node_Coord_y(connectivity(2));
    node2_z=p.node_Coord_z(connectivity(2));
    
    elem_length=sqrt((node1_x-node2_x).^2+(node1_y-node2_y).^2+(node1_z-node2_z).^2);
    Cx=(node2_x-node1_x)/elem_length;
    Cy=(node2_y-node1_y)/elem_length;
    Cz=(node2_z-node1_z)/elem_length;

    T=[Cx*Cx Cx*Cy Cx*Cz;
        Cx*Cy Cy*Cy Cy*Cz;
        Cx*Cz Cy*Cz Cz*Cz];

    k_e=(p.E*p.A/elem_length)*[T -T;-T T];
    p.elemStiffs(:,:,i)=k_e;

    % assemble stiffness matrix
    p.global_stiffness_matrix(elem_dof,elem_dof)=...
        p.global_stiffness_matrix(elem_dof,elem_dof)+k_e;

end

% apply boundary condition 
p.displacements(p.fix_dof)=0;
p.node_forces(p.load_dof)=p.P;

% solution KU=F
p = solutionStruct(p);

% calculate stress
p.stress=zeros(p.elem_num,1);
for i=1:p.elem_num
    connectivity=p.elems(i,:);
    % elem's all dof : a 3d truss node has 2 node,each node have 3 dof,
    % i-th node --> U_(3*i-2) / U_(3*i-1) / U_(3*i) dof(displacement of node) 
    elem_dof=[connectivity(1)*3-2 connectivity(1)*3-1 connectivity(1)*3 ...
        connectivity(2)*3-2 connectivity(2)*3-1 connectivity(2)*3];

    node1_x=p.node_Coord_x(connectivity(1));
    node1_y=p.node_Coord_y(connectivity(1));
    node1_z=p.node_Coord_z(connectivity(1));
    node2_x=p.node_Coord_x(connectivity(2));
    node2_y=p.node_Coord_y(connectivity(2));
    node2_z=p.node_Coord_z(connectivity(2));
    
    elem_length=sqrt((node1_x-node2_x).^2+(node1_y-node2_y).^2+(node1_z-node2_z).^2);
    Cx=(node2_x-node1_x)/elem_length;
    Cy=(node2_y-node1_y)/elem_length;
    Cz=(node2_z-node1_z)/elem_length;

    p.stress(i)=(p.E/elem_length)*[-Cx -Cy -Cz Cx Cy Cz]*p.displacements(elem_dof);
end

% show compute result
disp('displacements of dofs :')
disp([(1:p.global_dof_num)', p.displacements])

disp('forces of dofs :')
disp([(1:p.global_dof_num)',p.node_forces])

disp('3d truss elem stress :')
disp([(1:p.elem_num)',p.stress])

figure
axis equal
view([1 1 1])
% plot undeformed mesh
draw_3Dtruss_mesh(p.nodes,p)

% plot deformed mesh
deformed_node_coordinate=cal_deformed_nodes(p,400.0);
disp('deformed nodes:')
disp(deformed_node_coordinate)

draw_3Dtruss_mesh(deformed_node_coordinate,p)

title('位移变形')
grid on
zlabel('Z')
ylabel('Y')
xlabel('X')

function draw_3Dtruss_mesh(nodes,p)
    xy=zeros(size(p.elems,2)*p.elem_num,p.problem_dimension);
    cur1=1;
    cur2=0;
    for i =1:p.elem_num
        
        connectivity=p.elems(i,:);
        attach_node_num=size(connectivity,2);
        
        cur1=cur2+1;
        cur2=cur2+attach_node_num;
        
        coord=zeros(attach_node_num,p.problem_dimension);
        for j=1:size(connectivity,2)
            coord(j,:)=nodes(connectivity(j),:);
        end
    
        xy(cur1:cur2,:)=coord;
    end
    line(xy(:,1),xy(:,2),xy(:,3),"Marker","o","LineWidth",1,"Color",rand(1,3))
end





















